#! /usr/bin/env python  
# coding=utf-8

import rospy
import numpy as np
import matplotlib.pyplot as plt
import matplotlib as mpl
import time
import math

pi = math.pi 


# mpl.rcParams['font.sans-serif'] = ['SimHei']  # 指定默认字体
# mpl.rcParams['axes.unicode_minus'] = False  # 解决保存图像是负号'-'显示为方块的问题


class QUARTIC_SCURVE:
    def __init__(self,Dmax=0.0,Vmax=0.0,Amax=0.0,Jmax=0.0,Smax=0.0):
        "位移，速度，加速度，加加速度，加加速度变化率"
       
        # 运动学约束
        self.Dmax = float(Dmax)
        self.Vmax = float(Vmax)
        self.Amax = float(Amax)
        self.Jmax = float(Jmax)
        self.Smax = float(Smax)
       
        # 实际运动学值
        self.V_max = 0.0
        self.A_max = 0.0
        self.J_max = 0.0
       
        # 时间段
        self.Ts = 0.0
        self.Tj = 0.0
        self.Ta = 0.0
        self.Tv = 0.0

        # 轨迹执行时间
        self.Tcost = 0.0
      
        # 时间点
        self.t1 = 0.0
        self.t2 = 0.0
        self.t3 = 0.0
        self.t4 = 0.0
        self.t5 = 0.0
        self.t6 = 0.0
        self.t7 = 0.0
        self.t8 = 0.0
        self.t9 = 0.0
        self.t10 = 0.0
        self.t11 = 0.0
        self.t12 = 0.0
        self.t13 = 0.0
        self.t14 = 0.0
        self.t15 = 0.0
    
        pass
    
    def updata_t(self):
        "更新所有时间点：t1-t15"
        self.t1 = self.Ts
        self.t2 = self.Ts + self.Tj
        self.t3 = 2*self.Ts + self.Tj
        self.t4 = 2*self.Ts + self.Tj + self.Ta
        self.t5 = 3*self.Ts + self.Tj + self.Ta
        self.t6 = 3*self.Ts + 2*self.Tj + self.Ta
        self.t7 = 4*self.Ts + 2*self.Tj + self.Ta
        self.t8 = 4*self.Ts + 2*self.Tj + self.Ta + self.Tv
        self.t9 = 5*self.Ts + 2*self.Tj + self.Ta + self.Tv
        self.t10 = 5*self.Ts + 3*self.Tj + self.Ta + self.Tv
        self.t11 = 6*self.Ts + 3*self.Tj + self.Ta + self.Tv
        self.t12 = 6*self.Ts + 3*self.Tj + 2*self.Ta + self.Tv
        self.t13 = 7*self.Ts + 3*self.Tj + 2*self.Ta + self.Tv
        self.t14 = 7*self.Ts + 4*self.Tj + 2*self.Ta + self.Tv
        self.t15 = 8*self.Ts + 4*self.Tj + 2*self.Ta + self.Tv
        
        pass
    
    
    def cal_Ts(self):
        "计算Smax持续时间Ts"
        
        Ts_d = np.power(0.125*self.Dmax/self.Smax,0.25)
        Ts_v = np.power(0.5*self.Vmax/self.Smax,1/3.0)
        Ts_a = np.power(self.Amax/self.Smax,0.5)
        Ts_j = self.Jmax/self.Smax
     
        self.Ts = np.min([Ts_d,Ts_v,Ts_a,Ts_j])
     
        if self.Ts == Ts_d:
            self.Tj = 0
            self.Ta = 0
            self.Tv = 0
        elif self.Ts == Ts_v:   
            self.Tj = 0
            self.Ta = 0
            self.cal_Tv()
        elif self.Ts == Ts_a:
            self.Tj = 0
            self.cal_Ta()
        else : # self.Ts == Ts_j
            self.cal_Tj()
        
        
        
        pass
    
    def cal_Tj(self):
        "计算Jmax持续时间Tj"
        
        Ts_3 = self.Ts*self.Ts*self.Ts
        Ts_2 = self.Ts*self.Ts
        Dmax_2 = self.Dmax*self.Dmax
        Jmax_2 = self.Jmax*self.Jmax
        
        Tj_d = np.power(Ts_3/27.0 + 0.25*self.Dmax/self.Jmax + 
                                np.power(self.Dmax*Ts_3/(54.0*self.Jmax) + 0.0625*Dmax_2/Jmax_2, 0.5),
                        1/3.0) + \
               np.power(Ts_3/27 + 0.25*self.Dmax/self.Jmax - 
                                np.power(self.Dmax*Ts_3/(54.0*self.Jmax) + 0.0625*Dmax_2/Jmax_2, 0.5), 
                        1/3.0) - 5*self.Ts/3.0
        
        
        Tj_v = np.power(0.25*Ts_2 + self.Vmax/self.Jmax,0.5) - 3*self.Ts/2.0
        
        Tj_a = self.Amax/self.Jmax - self.Ts
        
        self.Tj = np.min([Tj_d,Tj_v,Tj_a])
    

        if self.Tj == Tj_d:
            self.Tv = 0
            self.Ta = 0
        elif self.Tj == Tj_v:
            self.Ta = 0
            self.cal_Tv()
        else: # self.tj == Tj_a:
            self.cal_Ta()
        
        pass
    
    def cal_Ta(self):
        "计算Amax持续时间Ta"

        Ta_d = 0.5*( -(6.0*self.Ts+3.0*self.Tj) + 
                     np.power((2.0*self.Ts+self.Tj)*(2.0*self.Ts+self.Tj) + 4.0*self.Dmax/self.Amax, 0.5)  
                   )
     
        
    
        Ta_v = self.Vmax/self.Amax - self.Tj - 2.0*self.Ts
        
        self.Ta = np.min([Ta_d,Ta_v])
        
        
        if self.Ta == Ta_d:
            self.Tv = 0
        else: # self.Ta == Ta_v
            self.cal_Tv()
    
        pass
    
    def cal_Tv(self):
        "计算Vmax持续时间Tv"
        
        self.Tv = self.Dmax/self.Vmax - self.Ta - 2.0*self.Tj - 4.0*self.Ts
    
        pass
    
    
    def cal_Param(self):
        "根据约束计算所有时间段Tx及时间点tx和实际运动学值"
        
        self.cal_Ts()
        self.updata_t()
        
        self.Tcost = self.Tv + 2*self.Ta + 4*self.Tj + 8*self.Ts
        # self.t15 = self.Tcost
        
        self.J_max = self.Smax*self.Ts
        self.A_max = self.Smax*self.Ts*(self.Ts+self.Tj)
        self.V_max = self.Smax*self.Ts*(self.Ts+self.Tj)*(2*self.Ts+self.Tj+self.Ta)
        
        pass
    
    def Jerk(self, x):
        "加加速度曲线"
        
        if x <= self.t1:
            return self.Smax*x
        elif x <= self.t2:
            return self.Smax * self.t1
        elif x <= self.t3:
            return self.Smax*self.t1 - self.Smax*(x - self.t2)
        elif x <= self.t4:
            return 0
        elif x <= self.t5:
            return -self.Smax*(x - self.t4)
        elif x <= self.t6:
            return -self.Smax*self.t1
        elif x <= self.t7:
            return -self.Smax*self.t1 + self.Smax*(x - self.t6)
        elif x <= self.t8:
            return 0
        elif x <= self.t9:
            return -self.Smax*(x - self.t8)
        elif x <= self.t10:
            return -self.Smax*self.t1
        elif x <= self.t11:
            return -self.Smax*self.t1 + self.Smax*(x - self.t10)
        elif x <= self.t12:
            return 0
        elif x <= self.t13:
            return self.Smax*(x - self.t12)
        elif x <= self.t14:
            return self.Smax*self.t1
        elif x <= self.t15:
            return self.Smax*self.t1 - self.Smax*(x - self.t14)
        else:
            return 0
        
        pass
    
    def Acc(self, x):
        "加速度"
        
        a_t2 = 0.5*self.J_max*(self.Tj+self.t2)
        a_t3 = 0.5*self.J_max*(self.Tj+self.t3)
        
        if x <= self.t1:
            return 0.5*self.Smax*x*x
        elif x <= self.t2:
            return 0.5*self.J_max*(2*x-self.t1)
        elif x <= self.t3:
            return a_t2 + 0.5*(2*self.J_max-self.Smax*(x-self.t2))*(x-self.t2)
        elif x <= self.t4:
            return a_t3
        elif x <= self.t5:
            return a_t3 - 0.5*self.Smax*(x-self.t4)*(x-self.t4)
        elif x <= self.t6:
            return a_t3 - 0.5*self.J_max*(2*(x-self.t4)-self.t1)
        elif x <= self.t7:
            return 0.5*self.Smax*(self.t7-x)*(self.t7-x)
        elif x <= self.t8:
            return 0
        elif x <= self.t15:    
            return -self.Acc(x-self.t8)
        else:
            return 0
        
        pass
    
    def Acc2(self, x):
        "加速度"
        
        t1_2 = self.t1*self.t1
        t2_2 = self.t2*self.t2
        t3_2 = self.t3*self.t3
        t4_2 = self.t4*self.t4
        t5_2 = self.t5*self.t5
        t6_2 = self.t6*self.t6
        t7_2 = self.t7*self.t7
        
        if x <= self.t1:
            return 0.5*self.Smax*x*x
        elif x <= self.t2:
            return self.Smax*self.t1*x - 0.5*self.Smax*t1_2
        elif x <= self.t3:
            return self.Smax*( (self.t1+self.t2)*x - 0.5*x*x - 0.5*(t1_2+t2_2))
        elif x <= self.t4:
            return self.Smax*( -0.5*t3_2 + (self.t1+self.t2)*self.t3 - 0.5*(t1_2+t2_2))
        elif x <= self.t5:
            return self.Smax*( -0.5*x*x + self.t4*x + (self.t1+self.t2)*self.t3 - 0.5*(t1_2+t2_2+t3_2+t4_2))
        elif x <= self.t6:
            return self.Smax*( -self.t1*x + (self.t1+self.t4)*self.t5 
                               + (self.t1+self.t2)*self.t3 - 0.5*(t1_2+t2_2+t3_2+t4_2+t5_2)
                             )
        elif x <= self.t7:
            return self.Smax*( 0.5*x*x - (self.t1+self.t6)*x + (self.t1+self.t4)*self.t5 
                               + (self.t1+self.t2)*self.t3 - 0.5*(t1_2+t2_2+t3_2+t4_2+t5_2-t6_2)
                              )
        elif x <= self.t8:
            return self.Smax*( 
                               -(self.t1+self.t6)*self.t7 + 
                               (self.t1+self.t4)*self.t5 + 
                               (self.t1+self.t2)*self.t3 - 
                               0.5*(t1_2+t2_2+t3_2+t4_2+t5_2-t6_2-t7_2)
                              )
        elif x <= self.t9:    
            return -self.Acc(x-self.t8)
        elif x <= self.t10:    
            return -self.Acc(x-self.t8)
        elif x <= self.t11:    
            return -self.Acc(x-self.t8)
        elif x <= self.t12:    
            return -self.Acc(x-self.t8)
        elif x <= self.t13:    
            return -self.Acc(x-self.t8)
        elif x <= self.t14:    
            return -self.Acc(x-self.t8)
        elif x <= self.t15:    
            return -self.Acc(x-self.t8)
        else:
            return 0
        
        pass
    
    def Vel(self, x):
        "速度"
        
        x3 = x*x*x
        t1_2 = self.t1*self.t1
        t1_3 = self.t1*self.t1*self.t1
        t2_2 = self.t2*self.t2
        t2_3 = self.t2*self.t2*self.t2
        
        t3_2 = self.t3*self.t3
        t3_3 = self.t3*self.t3*self.t3
        

        
        
        if x <= self.t1:
            return self.Smax*x3/6.0
        elif x <= self.t2:
            return self.Smax*( 0.5*self.t1*x*x - 0.5*t1_2*x + t1_3/6.0 )
        elif x <= self.t3:
            return self.Smax*(  -x3/6 + 0.5*(self.t1+self.t2)*x*x - 0.5*(t1_2+t2_2)*x + (t1_3+t2_3)/6.0 )
        elif x <= self.t4:
            return self.Smax*( 0.5*(self.t1+self.t2)*t3_2 - 0.5*(t1_2+t2_2)*self.t3 + (t1_3+t2_3-t3_3)/6.0 ) \
                   + self.A_max*(x-self.t3)
        elif x <= self.t7: # 根据加速段中心对称计算
            vel_x_ = self.t3 + 0.5*self.Ta # 速度曲线加速段对称点
            vel_y_ = self.Smax*( 0.5*(self.t1+self.t2)*t3_2 - 0.5*(t1_2+t2_2)*self.t3 + (t1_3+t2_3-t3_3)/6.0 ) \
                    + self.A_max*(vel_x_-self.t3)
            return 2*vel_y_ - self.Vel(2*vel_x_ - x)
        elif x <= self.t8:
            return self.V_max
        elif x <=self.t15: # 根据速度曲线加减速段对称计算
            vel_x__ = self.t7 + 0.5*self.Tv # 速度曲线对称轴
            return self.Vel(2*vel_x__ - x)
        else:
            return 0
        
        pass
    
    def Vel2(self,x):
        
        x3 = x*x*x
        t1_2 = self.t1*self.t1
        t1_3 = self.t1*self.t1*self.t1
        t2_2 = self.t2*self.t2
        t2_3 = self.t2*self.t2*self.t2
        t3_2 = self.t3*self.t3
        t3_3 = self.t3*self.t3*self.t3
        t4_2 = self.t4*self.t4
        t4_3 = self.t4*self.t4*self.t4
        t5_2 = self.t5*self.t5
        t5_3 = self.t5*self.t5*self.t5
        t6_2 = self.t6*self.t6
        t6_3 = self.t6*self.t6*self.t6
        
        if x <= self.t1:
            return self.Smax*x3/6
        elif x <= self.t2:
            return self.Smax*( 0.5*self.t1*x*x - 0.5*t1_2*x + t1_3/6 )
        elif x <= self.t3:
            return self.Smax*(  -x3/6 + 0.5*(self.t1+self.t2)*x*x - 0.5*(t1_2+t2_2)*x + (t1_3+t2_3)/6 )
        elif x <= self.t4:
            return self.Smax*( ((self.t1+self.t2)*self.t3 - 0.5*(t1_2+t2_2+t3_2))*x + 
                               (t1_3+t2_3-t3_3)/6 + 0.5*t3_3 - 0.5*(self.t1+self.t2)*t3_2  
                              ) 
        elif x <= self.t5: 
            return self.Smax*( 0.5*self.t4*x*x - x3/6 - 0.5*(t4_2+t3_2+t2_2+t1_2)*x + 
                               (self.t1+self.t2)*self.t3*x + (t1_3+t2_3-t3_3+t4_3)/6 + 0.5*t3_3 - 
                                  0.5*(self.t1+self.t2)*t3_2
                              )
        elif x <= self.t6: 
            return self.Smax*( -0.5*self.t1*x*x + (self.t1+self.t4)*self.t5*x 
                               - 0.5*(t5_2+t4_2+t3_2+t2_2+t1_2)*x + (self.t1+self.t2)*self.t3*x
                                + (t1_3+t2_3+t4_3)/6 + (t3_3+t5_3)/3 
                                - 0.5*(self.t1+self.t4)*t5_2 - 0.5*(self.t1+self.t2)*t3_2
                              )
        elif x <= self.t7: 
            return self.Smax*( -0.5*(self.t1+self.t6)*x*x + x3/6 +  (self.t1+self.t4)*self.t5*x 
                               - 0.5*(-t6_2+t5_2+t4_2+t3_2+t2_2+t1_2)*x 
                               + (self.t1+self.t2)*self.t3*x
                               + (t1_3+t2_3+t4_3-t6_3)/6 + (t3_3+t5_3)/3 
                               - 0.5*(self.t1+self.t4)*t5_2 - 0.5*(self.t1+self.t2)*t3_2
                              )
        elif x <= self.t8:
            return self.V_max
        elif x <= self.t15:
            vel_x__ = self.t7 + 0.5*self.Tv
            return self.Vel2(2*vel_x__ - x)
        else:
            return 0
        
        
        pass
    
    def Dis(self, x):
        "位移"
        
        x4 = x*x*x*x
        x3 = x*x*x
        x2 = x*x
        t1_2 = self.t1*self.t1
        t1_3 = self.t1*self.t1*self.t1
        t1_4 = self.t1*self.t1*self.t1*self.t1
        t2_2 = self.t2*self.t2
        t2_3 = self.t2*self.t2*self.t2
        t2_4 = self.t2*self.t2*self.t2*self.t2
        t3_2 = self.t3*self.t3
        t3_3 = self.t3*self.t3*self.t3
        t3_4 = self.t3*self.t3*self.t3*self.t3
        t4_2 = self.t4*self.t4
        t4_3 = self.t4*self.t4*self.t4
        t4_4 = self.t4*self.t4*self.t4*self.t4
        t5_2 = self.t5*self.t5
        t5_3 = self.t5*self.t5*self.t5
        t5_4 = self.t5*self.t5*self.t5*self.t5
        t6_2 = self.t6*self.t6
        t6_3 = self.t6*self.t6*self.t6
        t6_4 = self.t6*self.t6*self.t6*self.t6
        
        if x <= self.t1:
            return self.Smax*x4/24.0
        elif x <= self.t2:
            return self.Smax*( self.t1*x3/6.0 - 0.25*t1_2*x2 + t1_3*x/6 - t1_4/24.0 )
        elif x <= self.t3:
            return self.Smax*( - x4/24.0 + (self.t1+self.t2)*x3/6.0 - 0.25*(t1_2+t2_2)*x2 + (t1_3+t2_3)*x/6.0 
                               - (t1_4+t2_4)/24.0 
                            )
        elif x <= self.t4:
            return self.Smax*(  0.5*(self.t1+self.t2)*self.t3*x2 - 0.25*(t1_2+t2_2+t3_2)*x2
                              + (t1_3+t2_3)*x/6.0 + t3_3*x/3.0 - 0.5*(self.t1+self.t2)*t3_2*x
                              - (t1_4+t2_4)/24.0 - 0.125*t3_4 + (self.t1+self.t2)*t3_3/6.0
                            )
        elif x <= self.t5:
            return self.Smax*( self.t4*x3/6 - x4/24  - 0.25*(t1_2+t2_2+t3_2+t4_2)*x2
                              + 0.5*(self.t1+self.t2)*self.t3*x2 + (t1_3+t2_3+t4_3)*x/6.0
                               + t3_3*x/3 - 0.5*(self.t1+self.t2)*t3_2*x
                              - (t1_4+t2_4+t4_4)/24.0 - 0.125*t3_4 + (self.t1+self.t2)*t3_3/6.0
                            )
        elif x <= self.t6:
            return self.Smax*(-self.t1*x3/6 + 0.5*(self.t1+self.t4)*self.t5*x2
                              - 0.25*(t1_2+t2_2+t3_2+t4_2+t5_2)*x2 + 0.5*(self.t1+self.t2)*self.t3*x2
                              + (t1_3+t2_3+t4_3)*x/6 + (t3_3+t5_3)*x/3 
                              - 0.5*(self.t1+self.t4)*t5_2*x - 0.5*(self.t1+self.t2)*t3_2*x
                              - (t1_4+t2_4+t4_4)/24 - 0.125*(t3_4+t5_4) 
                              + (self.t1+self.t4)*t5_3/6 + (self.t1+self.t2)*t3_3/6
                            )
        elif x <= self.t7:
            return self.Smax*(-(self.t1+self.t6)*x3/6 + x4/24 + 0.5*(self.t1+self.t4)*self.t5*x2
                              - 0.25*(t1_2+t2_2+t3_2+t4_2+t5_2-t6_2)*x2 
                              + 0.5*(self.t1+self.t2)*self.t3*x2
                              + (t1_3+t2_3+t4_3-t6_3)*x/6 + (t3_3+t5_3)*x/3 
                              - 0.5*(self.t1+self.t4)*t5_2*x - 0.5*(self.t1+self.t2)*t3_2*x
                              - (t1_4+t2_4+t4_4-t6_4)/24 - 0.125*(t3_4+t5_4) 
                              + (self.t1+self.t4)*t5_3/6 + (self.t1+self.t2)*t3_3/6
                            )
        elif x <= self.t8:
            return self.Dis(self.t7) + self.V_max*(x - self.t7)
        elif x <= self.t15: # 根据中心对称性计算
            dis_x_ = self.t7 + 0.5*self.Tv
            dis_y_ = self.Dis(self.t7) + self.V_max*0.5*self.Tv
            return 2*dis_y_ - self.Dis(2*dis_x_ - x)
        else: # 防止系统出错，重复最后的计算结果
            print('err x > self.t15 or x < 0')
            x = self.t15
            dis_x_ = self.t7 + 0.5*self.Tv
            dis_y_ = self.Dis(self.t7) + self.V_max*0.5*self.Tv
            return 2*dis_y_ - self.Dis(2*dis_x_ - x)
        
        
        
        pass
    
    def get_interPoly_Dis(self, poly_num):
        """ 输出位置插补结果 """
        x = np.linspace(0,self.Tcost,int(poly_num))
        

        return np.vectorize(self.Dis)(x)

        pass
    
    def plot_quarticCurve(self,poly_num, show=1):
        "画图"
        
        x = np.linspace(0,self.Tcost,int(poly_num))
        j = np.vectorize(self.Jerk)(x)
        a = np.vectorize(self.Acc)(x)
        v = np.vectorize(self.Vel)(x)
        # v2 = np.vectorize(self.Vel2)(x)
        d = np.vectorize(self.Dis)(x)

        # j = np.zeros(int(poly_num), np.float)
        # a = np.zeros(int(poly_num), np.float)
        # v = np.zeros(int(poly_num), np.float)
        # d = np.zeros(int(poly_num), np.float)
        # for i in range(0,int(poly_num)):
        #     j[i] = self.Jerk(x[i])
        #     a[i] = self.Jerk(x[i])
        #     v[i] = self.Jerk(x[i])
        #     d[i] = self.Jerk(x[i])
            
        #     pass


        # print('d: ',j)
      
        print('Ts',self.Ts,'Tj',self.Tj,'Ta',self.Ta,'Tv',self.Tv)
        print('Vmax',self.V_max,'Amax',self.A_max,'Jmax',self.J_max)
    
        print('Tcost',self.Tcost)
        
        plt.figure()
        plt.subplot(4,1,1)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.ylabel('j(t)')
        plt.grid(True)
        plt.plot(x,j)
        
        plt.subplot(4,1,2)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.ylabel('a(t)')
        plt.grid(True)
        plt.plot(x,a)
        
        plt.subplot(4,1,3)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.ylabel('v(t)')
        plt.grid(True)
        plt.plot(x,v)
        # plt.plot(x,v2,c='r')
        
        plt.subplot(4,1,4)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.ylabel('d(t)')
        plt.xlabel('t/(s)')
        plt.grid(True)
        plt.plot(x,d)
        
        # plt.autoscale(True, axis='both')

        if show:
            plt.show()
    
        pass
    


    pass


def quartic_TimePoly(s_pose,e_pose,Vmax,Amax,Jmax,Smax,poly_num=None,poly_rate=None,Dmax=np.array([0,0,0,0,0,0])):
    "四次多项式曲线 时间同步规划"

    sTime = time.time() # us
    if poly_num==None and poly_rate==None:
        print("err: poly_num==None and poly_rate==None \n")
        return
    
    dim = len(s_pose)
    if dim==len(e_pose) and dim==len(Vmax) and dim==len(Amax) and dim==len(Jmax) and dim ==len(Smax) :
        pass
    else :
        print('err,param dim not same\n')
        return
    
    Dmax = np.abs(e_pose-s_pose)
    
    zero_Dindex = [] # 0位移自由度序号
    for i in range(dim):
        if Dmax[i] <= 0.000001:
            zero_Dindex.append(i) # 位移为0的执行器序号
    
    qobj_List = []
    # d_List = np.zeros((poly_num,dim)) 
    t_List = np.zeros((dim,1))
    maxT_index = 0
    max_Tcost = 0
    for i in range(dim):
        if Dmax[i] >= 0.000001: # 位移不为0
            qobj = QUARTIC_SCURVE(Dmax[i],Vmax[i],Amax[i],Jmax[i],Smax[i])
            qobj.cal_Param()
            qobj_List.append(qobj)
            t_List[i,0] = qobj.Tcost
            # x = np.linspace(0,qobj.Tcost,poly_num) 
            # d_List[:,i] = np.vectorize((qobj).Dis)(x)
            # if e_pose[i]<s_pose[i]: # 位移为负
            #     d_List[:,i] = -d_List[:,i]
            if max_Tcost < qobj.Tcost: # 获取同步时间
                max_Tcost = qobj.Tcost
                maxT_index = i
        else: # 位移为0
            qobj_List.append(0)
            t_List[i,0] = 0
            pass

    poly_num = poly_num if poly_rate==None else int(max_Tcost*poly_rate)
    d_List = np.zeros((poly_num,dim)) 
    for i in range(dim):
        if Dmax[i] >= 0.000001: # 位移不为0
            x = np.linspace(0,t_List[i,0],poly_num) 
            d_List[:,i] = np.vectorize((qobj_List[i]).Dis)(x)
            if e_pose[i]<s_pose[i]: # 位移为负
                d_List[:,i] = -d_List[:,i]


    # 加上起始位置
    d_List = d_List + np.tile(np.reshape(s_pose,(1,dim)),(poly_num,1))
    
    eTime = time.time() # us
    
    print('compute time {}ms'.format((eTime-sTime)*1000))
    print('syncTime:{}s'.format(max_Tcost))
    
    
    return d_List,max_Tcost
    pass

def plot_from_data(mode='Jdeg',indexToPlot=[0],t=1.0,data=None, defualt_xticks=0):
    
    if mode=='Jdeg':
        if data.any()!=None:
            Jdeg=data
        else:
            print("err:  no data")
            return
            # Jdeg, _ = getData_from_txt()
            
        row,cul = Jdeg.shape
     
        plot_point = int(row) # 要画出的插值点数
        
        # row = 980 # 修改要显示的插值点数
        # Jdeg = Jdeg[10:row+10,:]
        
        
        plot_num = len(indexToPlot)
        
        x = np.linspace(0,t,row)
        y = np.zeros((row,plot_num))
        dy = np.zeros((row,plot_num))
        ddy = np.zeros((row,plot_num))
        dddy = np.zeros((row,plot_num))
        for i in range(plot_num):
            y[:,i] = Jdeg[:,indexToPlot[i]]
            dy[:,i] = np.gradient(y[:,i],x)
            ddy[:,i] = np.gradient(dy[:,i],x)
            dddy[:,i] = np.gradient(ddy[:,i],x)
        # dddy[0:10,:] = 0
        # dddy[990:1000,:] = 0
        

        linestyle_list=['-','-','--',':',':','--']
        plt.figure()
        plt.rcParams.update({'font.size': 9})     #设置图例字体
        # 子图1用于显示图例***********************************************
        plt.subplot(5,1,1) 
        ax = plt.gca() # get current axis
        ax.spines['right'].set_color('none')  # spines 是图的四周边框
        ax.spines['top'].set_color('none')
        ax.spines['left'].set_color('none')
        ax.spines['bottom'].set_color('none')
        # plt.axes().get_xaxis().set_visible(False) # 隐藏x坐标轴
        # plt.axes().get_yaxis().set_visible(False) # 隐藏y坐标轴
        plt.yticks([100],
                [''] # 使用正则表达式加转译 （latex公式）                   
                ) 
        plt.xticks([100],
                [''] # 使用正则表达式加转译 （latex公式）                   
                ) 
        plt.ylim((300,301))
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],label='aixs{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])
            plt.legend( ncol=3, # 图例显示几列，默认为1
                    loc='lower center' # 图例位置(默认best 为自动调整图例位置)
                    ) # 显示自定义格式图例
        
        

        plt.subplot(5,1,2)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('位移')
        plt.ylabel(r'$d/\rm rad$')
        if not defualt_xticks:
            plt.yticks([-1,0,1,2],
            ['-1.0','0','1.0','2.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
            ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],label='aixs{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])
            # plt.legend( ncol=3, # 图例显示几列，默认为1
            #         loc='best' # 图例位置(默认best 为自动调整图例位置)
            #         ) # 显示自定义格式图例
            
        # plt.figure()
        plt.subplot(5,1,3)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('速度')
        plt.ylabel(r'$v/\rm (rad/s)$')
        if not defualt_xticks:
            plt.yticks([-0.5,0,0.5,1.0,1.45],
            ['-0.5','0','0.5','1.0','1.5'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
            ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],dy[0:plot_point,i],linestyle=linestyle_list[i])
            
        # plt.figure()
        plt.subplot(5,1,4)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加速度')
        # plt.ylabel(r'$a/(rad{\cdot}s^{-2})$')
        plt.ylabel(r'$a/\rm (rad/s^{2})$')
        if not defualt_xticks:
            plt.yticks([-5.0,-2.5,0,2.5,5.0],
            ['-5.0','-2.5','0','2.5','5.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
            ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],ddy[0:plot_point,i],linestyle=linestyle_list[i])
            
        # plt.figure()
        plt.subplot(5,1,5)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加加速度')
        plt.ylabel(r'$j/\rm (rad/s^{3})$')
        plt.xlabel('time/{}'.format(r'$s$'))
        if not defualt_xticks:
            plt.yticks([-20,0,20],
            ['-20.0','0','20.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
            ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],dddy[0:plot_point,i],linestyle=linestyle_list[i])
      
        print('tcost',t)
        
        plt.show()
        pass
    elif mode=='pose':
        if  type(data)==type(None):
            print("err: no data")
            return
            # Jdeg = get_xyzEulerZYX()
        elif data.any()!=None:
            Jdeg=data
        else:
            pass
            
        row,cul = Jdeg.shape
        
        plot_point = int(row) # 要画出的插值点数
        plot_num = len(indexToPlot)
        
        x = np.linspace(0,t,row)
        y = np.zeros((row,plot_num))
        dy = np.zeros((row,plot_num))
        ddy = np.zeros((row,plot_num))
        dddy = np.zeros((row,plot_num))
        for i in range(plot_num):
            y[:,i] = Jdeg[:,indexToPlot[i]]
            dy[:,i] = np.gradient(y[:,i],x)
            ddy[:,i] = np.gradient(dy[:,i],x)
            dddy[:,i] = np.gradient(ddy[:,i],x)
        # dddy[0:10,:] = 0
        # dddy[990:1000,:] = 0
    
        linestyle_list=['-','-','--',':',':','--'] 
        plt.figure()
        plt.rcParams.update({'font.size': 9})     #设置图例字体
        # 子图1用于显示图例****************************************
        plt.subplot(5,1,1) 
        ax = plt.gca() # get current axis
        ax.spines['right'].set_color('none')  # spines 是图的四周边框
        ax.spines['top'].set_color('none')
        ax.spines['left'].set_color('none')
        ax.spines['bottom'].set_color('none')
        # plt.axes().get_xaxis().set_visible(False) # 隐藏x坐标轴
        # plt.axes().get_yaxis().set_visible(False) # 隐藏y坐标轴
        if not defualt_xticks:
            plt.yticks([100],
                    [''] # 使用正则表达式加转译 （latex公式）                   
                    ) 
            plt.xticks([100],
                    [''] # 使用正则表达式加转译 （latex公式）                   
                    ) 
            plt.ylim((300,301))
        label_font=['x','y','z']
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],label='{}axis'.format(label_font[i]),linestyle=linestyle_list[i])
            plt.legend( ncol=3, # 图例显示几列，默认为1
                    loc='lower center' # 图例位置(默认best 为自动调整图例位置)
                    ) # 显示自定义格式图例
      
        #位移*************************************************************
        plt.subplot(5,1,2)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('位移')
        plt.ylabel(r'$d/\rm mm$')
        if not defualt_xticks:
            # plt.yticks([0,25,50],
            # ['0','25','50'] # 使用正则表达式加转译 （latex公式）                   
            # ) 
            plt.xticks([0,0.15,0.3,0.45,0.597],
            ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],linestyle=linestyle_list[i])
            # plt.legend( ncol=3, # 图例显示几列，默认为1
            #         loc='best' # 图例位置(默认best 为自动调整图例位置)
            #         ) # 显示自定义格式图例
        
        #速度***************************************************************
        plt.subplot(5,1,3)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('速度')
        plt.ylabel(r'$v/\rm (mm/s)$')
        # plt.xlim((-1,2))
        if not defualt_xticks:
            plt.ylim((-5,55)) 
            plt.yticks([0,25,50],
            ['0','25','50'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.15,0.3,0.45,0.597],
            ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],dy[0:plot_point,i],linestyle=linestyle_list[i])
        
        #加速度***************************************************************  
        plt.subplot(5,1,4)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加速度')
        # plt.ylabel(r'$a/(mm{\cdot}s^{-2})$')
        plt.ylabel(r'$a/\rm (mm/s^{2})$')
        if not defualt_xticks:
            plt.ylim((-450,450))
            plt.yticks([-400,-200,0,200,400],
            ['-400','-200','0','200','400'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.15,0.3,0.45,0.597],
            ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],ddy[0:plot_point,i],linestyle=linestyle_list[i])
        
        #加加速度**************************************************************
        plt.subplot(5,1,5)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加加速度')
        plt.ylabel(r'$j/\rm (mm/s^{3})$')
        plt.xlabel('time/{}'.format(r'$s$'))
        if not defualt_xticks:
            # plt.yticks([-20,0,20],
            # ['-20.0','0','20.0'] # 使用正则表达式加转译 （latex公式）                   
            # ) 
            plt.xticks([0,0.15,0.3,0.45,0.597],
            ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],dddy[0:plot_point,i],linestyle=linestyle_list[i])
      
        print('tcost',t)
        
        plt.show()
        pass
    pass


def quartic_test():

    s_time = time.time()
    
    q = QUARTIC_SCURVE(0.1, 30, 90, 1000, 10000)
    q.cal_Param()
    q.plot_quarticCurve(1000, show=0)
    polyD = q.get_interPoly_Dis(poly_num=100)
    
    e_time = time.time()
    print("time: %.2fms"%((e_time-s_time)*1000))
    print("rate:%.3f"%(1/(e_time-s_time)))
    print("polyD", polyD)

    
    q.Dmax = 10.0
    q.Vmax = 90.0
    q.Amax = 10.0
    q.Jmax = 1200.0
    q.Smax = 1200.0
    q.cal_Param()
    q.plot_quarticCurve(1000)
    polyD = q.get_interPoly_Dis(poly_num=100)


    pass

def quartic_joint_poly():

    s_pose = np.array([0,    0,  90,  0,  10,   0])
    e_pose = np.array([30, -30,   0,  0,  100,  90]) 
    Vmax = np.array([50,50,50,50,50,50]) 
    Amax = np.array([100,100,100,100,100,100]) 
    Jmax = np.array([3000,3000,3000,3000,3000,3000])
    Smax = np.array([9000,9000,9000,9000,9000,9000]) 
    Jdeg,Tcost = quartic_TimePoly(s_pose,e_pose,Vmax,Amax,Jmax,Smax,poly_num=1000, poly_rate=200)
    print("Jdeg.shape:",Jdeg.shape)
    plot_from_data(mode='Jdeg',indexToPlot=[0,1,2,3,4,5],t=Tcost,data=Jdeg, defualt_xticks=1)

    pass



if __name__=="__main__":
    rospy.init_node("quartic_scurve_py_node")

    quartic_joint_poly()


    pass
